#include "CanMessage.h"


#include "Serial.h"

#include <string.h>
#include <stdlib.h>
#include <can.h>
#include "MyCan.h"

#define CAN_RX_BUFFER_SIZE 8
static CANMessage RxBuffer[CAN_RX_BUFFER_SIZE];
static int inIndex;
static int outIndex;

void HandleCANRX1()
{
    CANMessage* bufferMessage = RxBuffer + inIndex;
    bufferMessage->ID = CAN1GetRecieveExtendedID1();
    bufferMessage->DataLength = CAN1GetDLC1();
    CAN1ReceiveMessage(bufferMessage->Data, bufferMessage->DataLength, 1);
    
    inIndex++;
    inIndex = inIndex & (CAN_RX_BUFFER_SIZE - 1);
}

void HandleCANRX0()
{
    CANMessage* bufferMessage = RxBuffer + inIndex;
    bufferMessage->ID = CAN1GetRecieveExtendedID0();
    bufferMessage->DataLength = CAN1GetDLC0();
    CAN1ReceiveMessage(bufferMessage->Data, bufferMessage->DataLength, 0);
    
    inIndex++;
    inIndex = inIndex & (CAN_RX_BUFFER_SIZE - 1);

}

int HasCANMessage()
{
        return inIndex != outIndex;
}

int GetCANMessage(CANMessage* outMessage)
{
        unsigned char retVal = 0;
    if(HasCANMessage())
    {
        retVal = 1;
        
        __builtin_disi(0x3FFF);

        CANMessage* bufferMessage = RxBuffer + outIndex;
        outMessage->DataLength = bufferMessage->DataLength;
        outMessage->ID = bufferMessage->ID;
        memcpy(outMessage->Data, bufferMessage->Data, 8);


        outIndex++;
        outIndex = outIndex & (CAN_RX_BUFFER_SIZE - 1);
        __builtin_disi(0);

    }
    return retVal;
}

void PrintCANMessage(CANMessage* message)
{
    Putchar('T');
    char buffer[10];
    unsigned int idPiece = message->ID >> 24;
    itoa(buffer, idPiece, 16);
    PutString(buffer);
    idPiece = (message->ID >> 16) & 0xff;
    itoa(buffer, idPiece, 16);
    PutString(buffer);
    idPiece = (message->ID >> 8) & 0xff;
    itoa(buffer, idPiece, 16);
    PutString(buffer);
    idPiece = (message->ID) & 0xff;
    itoa(buffer, idPiece, 16);
    PutString(buffer);
    itoa(buffer, message->DataLength, 10);
    PutString(buffer);
    int i;
    for(i = 0; i < message->DataLength; i++)
    {
        itoa(buffer, message->Data[i], 16);
        PutString(buffer);
    }
    Putchar('\r');
}
